#include "can_driver_node.h"
#include "stdio.h"

CanDriverNode::CanDriverNode(int argc, char** argv)
{
    ros::init(argc, argv, "can_driver");

    m_nh = new ros::NodeHandle;
    pub_VCU_TX2_ClampLiftStatus = m_nh->advertise<can_msgs::VCU_TX2_ClampLiftStatus>("can_msgs/VCU_TX2_ClampLiftStatus", 1);
    pub_VCU_TX2_ControlStatus = m_nh->advertise<can_msgs::VCU_TX2_ControlStatus>("can_msgs/VCU_TX2_ControlStatus",1);
    pub_VCU_TX2_Diag_AD = m_nh->advertise<can_msgs::VCU_TX2_Diag_AD>("can_msgs/VCU_TX2_Diag_AD",1);
    pub_VCU_TX2_Diag_Motor_Ampere = m_nh->advertise<can_msgs::VCU_TX2_Diag_Motor_Ampere>("can_msgs/VCU_TX2_Diag_Motor_Ampere",1);
    pub_VCU_TX2_Diag_Motor_Status = m_nh->advertise<can_msgs::VCU_TX2_Diag_Motor_Status>("can_msgs/VCU_TX2_Diag_Motor_Status",1);
    pub_VCU_TX2_Diag_Status = m_nh->advertise<can_msgs::VCU_TX2_Diag_Status>("can_msgs/VCU_TX2_Diag_Status",1);
    pub_VCU_TX2_MotorStatus = m_nh->advertise<can_msgs::VCU_TX2_MotorStatus>("can_msgs/VCU_TX2_MotorStatus",1);
    pub_Clamp_TX2_LevelSensor = m_nh->advertise<can_msgs::Clamp_TX2_LevelSensor>("can_msgs/Clamp_TX2_LevelSensor",1);

    sub_TX2_VCU_ClampLiftCmd = m_nh->subscribe<can_msgs::TX2_VCU_ClampLiftCmd>("can_msgs/TX2_VCU_ClampLiftCmd", 1, &CanDriverNode::callback_TX2_VCU_ClampLiftCmd, this);
    sub_TX2_VCU_ControlCmd = m_nh->subscribe<can_msgs::TX2_VCU_ControlCmd>("can_msgs/TX2_VCU_ControlCmd", 1, &CanDriverNode::callback_TX2_VCU_ControlCmd, this);
    sub_TX2_VCU_MotorCmd = m_nh->subscribe<can_msgs::TX2_VCU_MotorCmd>("can_msgs/TX2_VCU_MotorCmd", 1, &CanDriverNode::callback_TX2_VCU_MotorCmd, this);
    
    thread_canet = new ThreadCANETUDP("192.168.100.2", "4001", "9001");
    thread_canet->set_msg_handler_callback(this->msg_handler, this);
}

CanDriverNode::~CanDriverNode()
{
    delete thread_canet;
    delete m_nh;
}

void CanDriverNode::msg_handler(CAN_FRAME *frame, void* me_void)
{
    CanDriverNode* me = (CanDriverNode*)me_void;
    // printf("msg_handler: can_id=0x%3X\n", frame->id);
    switch(frame->id)
    {
        case 0x340:
            me->decode_VCU_TX2_MotorStatus(frame);
            break;
        case 0x341:
            me->decode_VCU_TX2_ControlStatus(frame);
            break;
        case 0x342:
            me->decode_VCU_TX2_ClampLiftStatus(frame);
            break;
        case 0x343:
            me->decode_VCU_TX2_Diag_Status(frame);
            break;
        case 0x344:
            me->decode_VCU_TX2_Diag_Motor_Status(frame);
            break;
        case 0x345:
            me->decode_VCU_TX2_Diag_Motor_Ampere(frame);
            break;
        case 0x346:
            me->decode_VCU_TX2_Diag_AD(frame);
            break;
        case 0x532:
            me->decode_Clamp_TX2_LevelSensor(frame);
            break;
    }
}

void CanDriverNode::decode_VCU_TX2_ClampLiftStatus(CAN_FRAME *frame)
{
    can_msgs::VCU_TX2_ClampLiftStatus msg;

    msg.FrontLeftWorkStatus = frame->data[0] & 0x03;
    msg.FrontRightWorkStatus = (frame->data[0]>>2) & 0x03;
    msg.RearLeftWorkStatus = (frame->data[0]>>4) & 0x03;
    msg.RearRightWorkStatus = (frame->data[0]>>6) & 0x03;

    msg.FrontLeftCmd = frame->data[1] & 0x03;
    msg.FrontRightCmd = (frame->data[1]>>2) & 0x03;
    msg.RearLeftCmd = (frame->data[1]>>4) & 0x03;
    msg.RearRightCmd = (frame->data[1]>>6) & 0x03;

    msg.FrontLeftPosition = (frame->data[2] & 0x1F) * 4;
    msg.FrontLeftReturn = (frame->data[2]>>5) & 0x01;
    msg.FrontLeft_RearSwitch = (frame->data[2]>>6) & 0x01;
    msg.FrontLeft_FrontSwitch = (frame->data[2]>>7) & 0x01;

    msg.FrontRightPosition = (frame->data[3] & 0x1F) * 4;
    msg.FrontRightReturn = (frame->data[3]>>5) & 0x01;
    msg.FrontRight_RearSwitch = (frame->data[3]>>6) & 0x01;
    msg.FrontRight_FrontSwitch = (frame->data[3]>>7) & 0x01;

    msg.RearLeftPosition = (frame->data[4] & 0x1F) * 4;
    msg.RearLeftReturn = (frame->data[4]>>5) & 0x01;
    msg.RearLeft_RearSwitch = (frame->data[4]>>6) & 0x01;
    msg.RearLeft_FrontSwitch = (frame->data[4]>>7) & 0x01;

    msg.RearRightPosition = (frame->data[5] & 0x1F) * 4;
    msg.RearRightReturn = (frame->data[5]>>5) & 0x01;
    msg.RearRight_RearSwitch = (frame->data[5]>>6) & 0x01;
    msg.RearRight_FrontSwitch = (frame->data[5]>>7) & 0x01;

    msg.LiftWorkStatus = (frame->data[6]) & 0x03;
    msg.LiftCmd = (frame->data[6]>>2) & 0x03;
    msg.LiftPosition = ((frame->data[6] >> 4) + (frame->data[7] << 4)) * 0.002;

    pub_VCU_TX2_ClampLiftStatus.publish(msg);
}

void CanDriverNode::decode_VCU_TX2_ControlStatus(CAN_FRAME *frame)
{
    can_msgs::VCU_TX2_ControlStatus msg;

    msg.ControlModeStatus = frame->data[0] & 0x03;
    msg.DrivingModeStatus = (frame->data[0]>>2) & 0x07;
    msg.ParkingBrakeStatus = (frame->data[0]>>5) & 0x01;
    msg.LeftSteerCmd = frame->data[1] * 1.0 - 128;
    msg.RightSteerCmd = frame->data[2] * 1.0 - 128;
    msg.LeftDriveCmd = frame->data[3] * 0.1 - 12.8;
    msg.RightDriveCmd = frame->data[4] * 0.1 - 12.8;

    pub_VCU_TX2_ControlStatus.publish(msg);
}

void CanDriverNode::decode_VCU_TX2_Diag_AD(CAN_FRAME *frame)
{
    can_msgs::VCU_TX2_Diag_AD msg;
    
    msg.PressureSensorAD = frame->data[0] + ((frame->data[1]&0x0F)<<8);
    msg.LevelSensorX = (frame->data[1]>>4) + (frame->data[2]<<4);
    msg.LevelSensorY = frame->data[3] + ((frame->data[4]&0x0F)<<8);
    
    pub_VCU_TX2_Diag_AD.publish(msg);
}

void CanDriverNode::decode_VCU_TX2_Diag_Motor_Ampere(CAN_FRAME *frame)
{
    can_msgs::VCU_TX2_Diag_Motor_Ampere msg;

    msg.LeftDriveAmpere = (short)(frame->data[0] + (frame->data[1]<<8)) * 0.1;
    msg.RightDriveAmpere = (short)(frame->data[2] + (frame->data[3]<<8)) * 0.1;
    msg.LeftSteerAmpere = (short)(frame->data[4] + (frame->data[5]<<8)) * 0.1;
    msg.RightSteerAmpere = (short)(frame->data[6] + (frame->data[7]<<8)) * 0.1;

    pub_VCU_TX2_Diag_Motor_Ampere.publish(msg);
}

void CanDriverNode::decode_VCU_TX2_Diag_Motor_Status(CAN_FRAME *frame)
{
    can_msgs::VCU_TX2_Diag_Motor_Status msg;

    msg.LeftStatusFlag = frame->data[0];
    msg.RightStatusFlag = frame->data[1];
    msg.SteerStatusFlag = frame->data[2];
    msg.LeftFaultFlag = frame->data[3];
    msg.RightFaultFlag = frame->data[4];
    msg.SteerFaultFlag = frame->data[5];
    msg.VBat = (frame->data[6] + (frame->data[7]<<8)) * 0.1;

    pub_VCU_TX2_Diag_Motor_Status.publish(msg);
}

void CanDriverNode::decode_VCU_TX2_Diag_Status(CAN_FRAME *frame)
{
    can_msgs::VCU_TX2_Diag_Status msg;

    msg.MotorFaultFlag = (frame->data[0]>>0) & 0x01;
    msg.MotorCommFaultFlag = (frame->data[0]>>1) & 0x01;
    msg.MotorInitFaultFlag = (frame->data[0]>>2) & 0x01;
    msg.LeftWheelLeftSwitch = (frame->data[0]>>3) & 0x01;
    msg.LeftWheelRightSwitch = (frame->data[0]>>4) & 0x01;
    msg.RightWheelLeftSwitch = (frame->data[0]>>5) & 0x01;
    msg.RightWheelRightSwitch = (frame->data[0]>>6) & 0x01;
    msg.TouchSwitch = (frame->data[0]>>7) & 0x01;

    msg.ClampFaultFlag = (frame->data[1]>>0) & 0x01;
    msg.ClampCommFaultFlag = (frame->data[1]>>1) & 0x01;
    msg.ClampStallFaultFlag = (frame->data[1]>>2) & 0x01;
    msg.ClampOvercurrentFaultFlag = (frame->data[1]>>3) & 0x01;
    msg.ClampSwitchFaultFlag = (frame->data[1]>>4) & 0x01;
    msg.ClampPositionSensorFaultFlag = (frame->data[1]>>5) & 0x01;

    msg.LiftFaultFlag = (frame->data[2]>>0) & 0x01;
    msg.LiftWorkingFlag = (frame->data[2]>>1) & 0x01;
    msg.TopLiftLimitSwitch = (frame->data[2]>>2) & 0x01;
    msg.BottomLiftLimitSwitch = (frame->data[2]>>3) & 0x01;

    msg.PanelFaultFlag = (frame->data[3]>>0) & 0x01;
    msg.PanelWorkingFlag = (frame->data[3]>>1) & 0x01;

    msg.WirelessFaultFlag = (frame->data[4]>>0) & 0x01;
    msg.WirelessWorkingFlag = (frame->data[4]>>1) & 0x01;
    
    msg.TX2FaultFlag = (frame->data[5]>>0) & 0x01;
    msg.TX2CommFaultFlag = (frame->data[5]>>1) & 0x01;

    pub_VCU_TX2_Diag_Status.publish(msg);
}

void CanDriverNode::decode_VCU_TX2_MotorStatus(CAN_FRAME *frame)
{
    can_msgs::VCU_TX2_MotorStatus msg;

    msg.LeftSteerPosition = (frame->data[0] + (frame->data[1]<<8)) * 0.01 - 300;
    msg.RightSteerPosition = (frame->data[2] + (frame->data[3]<<8)) * 0.01 - 300;
    msg.LeftDriveSpeed = (frame->data[4] + (frame->data[5]<<8)) * 0.001 - 32;
    msg.RightDriveSpeed = (frame->data[6] + (frame->data[7]<<8)) * 0.001 - 32;

    pub_VCU_TX2_MotorStatus.publish(msg);
}

void CanDriverNode::decode_Clamp_TX2_LevelSensor(CAN_FRAME *frame)
{
    can_msgs::Clamp_TX2_LevelSensor msg;

    msg.LevelSensorX = (frame->data[0] + ((frame->data[1]&0x0F)<<8)) * 5.0/4096.0;
    msg.LevelSensorY = (((frame->data[1]&0xF0)>>4) + (frame->data[2]<<4)) * 5.0/4096.0;

    pub_Clamp_TX2_LevelSensor.publish(msg);
}

void CanDriverNode::callback_TX2_VCU_ClampLiftCmd(const can_msgs::TX2_VCU_ClampLiftCmd::ConstPtr& msg)
{
    CAN_FRAME frame;

    frame.id = 0x432;
    frame.dlc = 8;
    frame.IDE = 0;
    frame.RTR = 0;
    memset(frame.data, 0, 8);
    frame.data[0] = (msg->ClampCmd & 0x03) + ((msg->LiftCmd & 0x01)<<2) + ((((unsigned short)(msg->LiftPositionCmd/0.002)) & 0x0F)<<4);
    frame.data[1] = ((((unsigned short)(msg->LiftPositionCmd/0.002)) & 0x0FF0)>>4);

    thread_canet->can_send(&frame);
}

void CanDriverNode::callback_TX2_VCU_ControlCmd(const can_msgs::TX2_VCU_ControlCmd::ConstPtr& msg)
{
    CAN_FRAME frame;
    
    frame.id = 0x431;
    frame.dlc = 8;
    frame.IDE = 0;
    frame.RTR = 0;
    memset(frame.data, 0, 8);
    frame.data[0] = msg->ParkingBrakeStatus & 0x01;;

    thread_canet->can_send(&frame);
}

void CanDriverNode::callback_TX2_VCU_MotorCmd(const can_msgs::TX2_VCU_MotorCmd::ConstPtr& msg)
{
    CAN_FRAME frame;
    
    frame.id = 0x430;
    frame.dlc = 8;
    frame.IDE = 0;
    frame.RTR = 0;
    memset(frame.data, 0, 8);
    frame.data[0] = ((unsigned short)((msg->LeftSteerPosition + 300.0) / 0.01)) & 0xFF;
    frame.data[1] = ((unsigned short)((msg->LeftSteerPosition + 300.0) / 0.01)) >>8;
    frame.data[2] = ((unsigned short)((msg->RightSteerPosition + 300.0) / 0.01)) & 0xFF;
    frame.data[3] = ((unsigned short)((msg->RightSteerPosition + 300.0) / 0.01)) >>8;
    frame.data[4] = ((unsigned short)((msg->LeftDriveSpeed + 32.0) / 0.001)) & 0xFF;
    frame.data[5] = ((unsigned short)((msg->LeftDriveSpeed + 32.0) / 0.001)) >>8;
    frame.data[6] = ((unsigned short)((msg->RightDriveSpeed + 32.0) / 0.001)) & 0xFF;
    frame.data[7] = ((unsigned short)((msg->RightDriveSpeed + 32.0) / 0.001)) >>8;

    thread_canet->can_send(&frame);
}

void CanDriverNode::loop()
{
    thread_canet->start();
    ros::spin();
}
